import rospy
from dynamic_reconfigure.client import Client

def set_d(new_d_value: float):
    rospy.init_node("pid_parameter_setter")
    controllers = [
        "FL_foot_controller",
        "FR_foot_controller",
        "RL_foot_controller",
        "RR_foot_controller"
    ]
    new_d_value = float(new_d_value)

    for controller in controllers:
        try:
            # 构建控制器的命名空间
            namespace = f"/newBee_gazebo/{controller}/pid"
            client = Client(namespace)
            # 更新 d 参数
            config = client.update_configuration({"d": new_d_value})
            rospy.loginfo(f"Set {namespace}/pid/d to {new_d_value}")
        except Exception as e:
            rospy.logerr(f"Failed to set d for {controller}: {e}")

def set_i(new_i_value: float):
    rospy.init_node("pid_parameter_setter")
    controllers = [
        "FL_foot_controller",
        "FR_foot_controller",
        "RL_foot_controller",
        "RR_foot_controller"
    ]
    new_i_value = float(new_i_value)

    for controller in controllers:
        try:
            # 构建控制器的命名空间
            namespace = f"/newBee_gazebo/{controller}/pid"
            client = Client(namespace)
            # 更新 i 参数
            config = client.update_configuration({"i": new_i_value})
            rospy.loginfo(f"Set {namespace}/pid/i to {new_i_value}")
        except Exception as e:
            rospy.logerr(f"Failed to set i for {controller}: {e}")

def set_p(new_p_value: float):
    rospy.init_node("pid_parameter_setter")
    controllers = [
        "FL_foot_controller",
        "FR_foot_controller",
        "RL_foot_controller",
        "RR_foot_controller"
    ]
    new_p_value = float(new_p_value)

    for controller in controllers:
        try:
            # 构建控制器的命名空间
            namespace = f"/newBee_gazebo/{controller}/pid"
            client = Client(namespace)
            # 更新 p 参数
            config = client.update_configuration({"p": new_p_value})
            rospy.loginfo(f"Set {namespace}/pid/p to {new_p_value}")
        except Exception as e:
            rospy.logerr(f"Failed to set p for {controller}: {e}")

def test():
    print("helloworld!")

